#ifndef ROBOT_LITE_DOG
#define ROBOT_LITE_DOG

#include "utils.h"
#include "modular.h"

#define Lite_Dog_Len 8
#define Lite_Dog_Pi 3.14159f
#define Lite_Dog_Angle_To_Digital 3.41f
#define Lite_Dog_Digital_TO_Angle 0.293f
#define	Lite_Dog_Gait_Cycle	1 //步态周期
#define Lite_Dog_Gait_Duty_Cycle 0.5f 	//支撑向占空比

typedef enum
{
	Lite_Dog_Forward = 1,
	Lite_Dog_Back,
	Lite_Dog_Step,
	Lite_Dog_Right,
	Lite_Dog_left,
	Lite_Dog_Stop,
}Lite_Dog_Motion_Mode; //运动模式枚举

typedef enum
{
  Lite_Dog_Trot = 1,
  Lite_Dog_Walk,
	Lite_Dog_Pronk,
}Lite_Dog_Gait_Mode; //模态模式枚举

typedef enum
{
  Lite_Dog_Remote_Control = 1,
  Lite_Dog_Calibration,
	Lite_Dog_Action
}Lite_Dog_Control_Mode; //控制模式枚举

typedef struct {
	float roll;
	float pitch;
	float yaw;
	float x;
	float z;
} Lite_Dog_Position_Attitude_Struct; //位置和姿态结构体

typedef struct {
	float x[4];
	float z[4];
} Lite_Dog_Foot_Coordinate_Struct; //足端坐标结构体

typedef struct {
	float thigh[4];
	float calf[4];
} Lite_Dog_Leg_Angle_Struct; //大小腿关节角结构体

typedef struct
{
	int servo_len; //舵机数量
	float servo_angle_range; //舵机角度范围
	float body_length; //身体长度
	float body_width; //身体宽度
	float foot_width; //足宽度
	float thigh_length; //大腿长度
	float calf_length; //小腿长度
	uint16_t zero_position[Lite_Dog_Len]; //校准后的零位数据
	int joint_offset; //关节偏移量
}Lite_Dog_Config_Struct;

typedef struct
{
	bool state; //状态
	Lite_Dog_Config_Struct config; //配置信息
	Lite_Dog_Control_Mode control_mode; //控制模式
	Lite_Dog_Motion_Mode motion_mode; //运动模式
	Lite_Dog_Gait_Mode gait_mode; //模态模式
	Lite_Dog_Position_Attitude_Struct target; //目标位置和姿态
	Lite_Dog_Position_Attitude_Struct current; //当前位置和姿态
	Lite_Dog_Foot_Coordinate_Struct foot_coordinate; //足端坐标
	Lite_Dog_Leg_Angle_Struct leg_angle; //大小腿关节角
	float gait_time; //步态时序时间
	uint32_t gait_cycle; //步态控制周期
	uint8_t auto_stable; //自稳
	Pid_Struct stable_pitch_pid; //俯仰自稳PID
	Pid_Struct stable_roll_pid; //俯仰滚转PID
	uint8_t auto_recovery; //自恢复
}Lite_Dog_Struct;

extern Lite_Dog_Struct Lite_Dog;

void Lite_Dog_Init(void);

void Lite_Dog_Attitude_To_Coordinate(const Lite_Dog_Position_Attitude_Struct *pose, float x[], float z[]);

void Lite_Dog_Coordinate_To_Angle(const float x[], const float z[], float thigh[], float calf[]);

void Lite_Dog_Data_To_Servo(float thigh[], float calf[], uint16_t zero_position[], uint16_t speed);

void Lite_Dog_Stand_Down(char *state);

void Lite_Dog_Motion(Lite_Dog_Position_Attitude_Struct *pose, uint16_t speed);

void Lite_Dog_Task(void);

#endif
